/**
 * 通用滑动块队列
 * 张勇 41204@qq.com
 */

#include <string.h>

#include "shift_block_queue.h"

// 注意: shift_block_t 要压缩存储，字段间无空隙 !!!
#pragma pack(push, 1)

/// @brief 块数据结构
/// @note struct 中位段顺序是由低位到高位排的，所以把 lock 标志放在后面
/// @note MSVC 不会让 uint32_t 和 uint8_t 位域混合存储，所以 type 和 lock 都要声明为 uint32_t。如果声明为 uint8_t，则 type 和 lock 不会与 len 共用一个 32 位，从而导致结构不是 32 位对齐的 !!!
typedef struct _shift_block {
	uint32_t len : 24;		///< data 有效数据长度
	uint32_t type: 7;   	///< 块类型
    uint32_t lock: 1;   	///< 加锁标志
	
    uint8_t  data[4];		///< 净荷: 有效数据长度为 len, 占用空间长度为 size (按 4 字节对齐的长度)	
    uint32_t size;			///< data 占用空间长度(在块末尾定义，以便逆向遍历队列)
} shift_block_t;

#pragma pack(pop)

/// @brief 块类型：标记为空闲的/可被删除的
#define SB_TYPE_FREE		(0x00)
/// @brief 块开销: 包含了 lock, type, len(位于块头部的长度指示), size(位于块尾部的长度指示) 几个字段的长度
#define SB_COST				(8)

/// @brief 计算指定净荷长度的块将占用的总空间字节数
#define SBSizeEval(len)     (SB_COST + (((len) + 3) & 0x00FFFFFC))
/// @brief 净荷按 4 字节对齐占用的空间字节数
#define SBDataSize(block)	((SBDataLen(block) + 3) & 0x00FFFFFC)
/// @brief 块占用的总空间字节数
#define SBSize(block)       (SBDataSize(block) + SB_COST)

// 正向遍历
/// @brief 指向 block 的末尾(下一个可能的块的头部): 返回 uint8_t*
#define SBNextAddr(block)   (((uint8_t*)block) + SBSize(block))
/// @brief 指向 block 的末尾(下一个可能的块的头部): 返回 shift_block_t*
#define SBNext(block)       ((shift_block_t*)SBNextAddr(block))

// 逆向遍历
/// @brief 指向 block 的前一个块的头部: 返回 uint8_t*
#define SBPrevAddr(block)   (((uint8_t*)block) - (*(((uint32_t*)block) - 1)) - SB_COST)
/// @brief 指向 block 的前一个块的头部: 返回 shift_block_t*
#define SBPrev(block)       ((shift_block_t*)SBPrevAddr(block))

shift_block_type_t SBType(shift_block_t* block) { return (((shift_block_t*)block)->type); }
uint8_t* SBData(shift_block_t* block)    		{ return (((shift_block_t*)block)->data); }
uint32_t SBDataLen(shift_block_t* block) 		{ return (((shift_block_t*)block)->len);  }
bool     SBIsFree(shift_block_t* block)			{ return (((shift_block_t*)block)->type == SB_TYPE_FREE); }
void     SBFree(shift_block_t* block)			{ (((shift_block_t*)block)->type = SB_TYPE_FREE); }
void     SBLock(shift_block_t* block)       	{ ((shift_block_t*)block)->lock = 1; }
void     SBUnlock(shift_block_t* block)			{ ((shift_block_t*)block)->lock = 0; }
bool     SBIsLocked(shift_block_t* block)   	{ return ((shift_block_t*)block)->lock == 1; }

bool SBIsValid(shift_block_queue_t* q, shift_block_t* block) {
	return ((uint8_t*)block) >= q->head &&	// 确保头在队列中
           SBNextAddr(block) <= q->tail;	// 确保尾在队列中
}

shift_block_t* SBQAlloc(shift_block_queue_t* q, uint8_t type, uint32_t len) {
    if(sizeof(shift_block_t) != 12) return NULL; // shift_block_t is unaligned !
	if(!q || type > SB_TYPE_MAX || len > SB_PAYLOAD_MAX) return NULL;
	
	// 在分配新块之前，先锁定队列，防止竞争冲突 !
	SBQLock(q);   //!!! critical {{{
	// 检查是否有足够的空间存放新的块
	if(q->tail + SBSizeEval(len) > q->limit) { SBQUnlock(q); return NULL; }
	
	// 返回的新块地址指向队尾
	shift_block_t* block = (shift_block_t*)(q->tail);
	// 设置块的头信息
	block->lock = 0;   	// 块锁标志
	block->type = type;	// 块类型
	block->len  = len;	// data 有效数据字节数
	// 返回新块指针
    return block;
}

shift_block_t* SBQCommit(shift_block_queue_t* q, shift_block_t* block) {
	// 设置块的尾部长度字段
	uint32_t len32 = (block->len + 3) >> 2; 	// data 占用空间字节数可存放多少个 uint32_t
	((uint32_t*)block)[1 + len32] = len32 << 2;	// data 占用空间字节数
	// 修改队列尾部位置
	q->tail += SBSize(block);
	// 解锁队列
	SBQUnlock(q); //!!! critical }}}
    return block;
}

shift_block_t* SBQAppend(shift_block_queue_t* q, uint8_t type, uint8_t len, uint8_t* payload) {
	// 分配块空间
	shift_block_t* block = SBQAlloc(q, type, len); if(!block) return NULL;
	// 拷贝净荷数据
	if(len && payload) memcpy(block->data, payload, len);
	// 提交块
	return SBQCommit(q, block);
}

shift_block_t* SBQNext(shift_block_queue_t* q, shift_block_t* current) {
    // 如果队列为空，则返回 NULL
    if(SBQIsEmpty(q)) { return NULL; }
	
    // 如果 current 不为 NULL 则取下一个; 否则从队列头开始找
	current = current ? SBNext(current) : (shift_block_t*)q->head;

    // 正向查找
    while(SBIsValid(q, current)) {
        // 如果非空，则返回
        if(!SBIsFree(current)) { return current; }
        // 取下一个
        current = SBNext(current);
    }

    // 没找到，返回 NULL
    return NULL;
}

// 正向删除从 begin 开始的所有空闲块，并滑动压缩空间
static void doPack(shift_block_queue_t* q, shift_block_t* begin, shift_block_t* last) {
    shift_block_t* follow = begin;
    shift_block_t* temp = NULL;

    while(SBIsValid(q, follow)) {
        if(SBIsFree(follow)) {
            follow = SBNext(follow); continue;
        }

        if(begin != follow) {
            temp = SBNext(follow);
            memcpy(begin, follow, SBSize(follow));
            follow = temp;
            begin = SBNext(begin);
            continue;
        } else {
            begin = follow = SBNext(follow);
        }
    }

    // 结束，设置队列尾
    q->tail = (uint8_t*)begin;
}

void SBQPack(shift_block_queue_t* q) {
    if(SBQIsEmpty(q)) { return; }

	SBQLock(q);   //!!! critical {{{

    // 指向最后一个锁定的末尾，默认是队列头部
    shift_block_t* begin = (shift_block_t*)q->head;
    // 指向最后一个非空非锁定的
    shift_block_t* last = NULL;
	// 是否有空隙
	uint8_t gap = 0;

    // 从末尾开始向前找 locked 和 last
    shift_block_t* block = SBPrev(q->tail);
    while(1) {
		if(!SBIsValid(q, block)) {
			break;
		}
		if(SBIsLocked(block)) {
            begin = SBNext(block); break; // 如果末尾有被锁定的，就不再压缩了
        }

        if(!last) {
            if(SBIsFree(block)) {
                // 如果是空闲的，则将队尾指向此块的头部（相当于删除此块）
                q->tail = (uint8_t*)block;
            } else {
                // 找到了最后一个未锁定，且不是空闲的
                last = block;
            }
		} else {
			if(SBIsFree(block)) {
				gap++;
			}
		}

        // 取前一个
        block = SBPrev(block);
	}

	// begin 表示开始压缩的起始地址
	// last 表示压缩的结束地址
	// gap 表示中间有多少个可被压缩的
    if(last && gap) {
    	// 如果后面有非空的，则从 begin 开始压缩
		doPack(q, begin, last);
	}

	SBQUnlock(q); //!!! critical }}}
}

void SBQClear(shift_block_queue_t* q) {
	SBQLock(q);   //!!! critical {{{
	q->tail = q->head;
	SBQUnlock(q); //!!! critical }}}
}


/**
// For TEST only !!!
void prepare(shift_block_queue_t* testq) {
    shift_block_t* block = NULL;

    uint8_t payload1[1]; memset(payload1, 0x11, 1);
    uint8_t payload2[2]; memset(payload2, 0x22, 2);
    uint8_t payload3[3]; memset(payload3, 0x33, 3);
    uint8_t payload4[4]; memset(payload4, 0x44, 4);
    uint8_t payload5[5]; memset(payload5, 0x55, 5);
    uint8_t payload6[6]; memset(payload6, 0x66, 6);
    uint8_t payload7[7]; memset(payload7, 0x77, 7);
    uint8_t payload8[8]; memset(payload8, 0x88, 8);
    uint8_t payload9[9]; memset(payload9, 0x99, 9);

    block = SBQAlloc(testq, 1);
    block->type = 0xF1;
    block->len = 1;
    block->payload[0] = 0x11;
    SBQCommit(testq, block);

    block = SBQAppend(testq, 0xF2, 2, payload2);
    block = SBQAppend(testq, 0xF3, 3, payload3);
    block = SBQAppend(testq, 0xF4, 4, payload4);
    block = SBQAppend(testq, 0xF5, 5, payload5);
    block = SBQAppend(testq, 0xF6, 6, payload6);
    block = SBQAppend(testq, 0xF7, 7, payload7);
    block = SBQAppend(testq, 0xF8, 8, payload8);
    block = SBQAppend(testq, 0xF9, 9, payload9);
}

void test1(shift_block_queue_t* testq) {
    shift_block_t* block = NULL;

    block = SBQNext(testq, block);
    block = SBQNext(testq, block); SBFree(block);
    block = SBQNext(testq, block); SBLock(block);
    block = SBQNext(testq, block); SBFree(block);
    block = SBQNext(testq, block);
    block = SBQNext(testq, block);
    block = SBQNext(testq, block); SBFree(block);
    block = SBQNext(testq, block);
    block = SBQNext(testq, block); SBUnlock(block);
    SBQPack(testq);

    block = SBQNext(testq, NULL); SBFree(block);
    block = SBNext(block);
    block = SBNext(block); SBUnlock(block);
    SBQPack(testq);

    block = SBQNext(testq, NULL); SBFree(block);
    block = SBNext(block); SBFree(block);
    block = SBNext(block); SBFree(block);
    SBQPack(testq);

    block = SBQNext(testq, NULL); SBFree(block); SBQPack(testq);
    block = SBQNext(testq, NULL); SBFree(block); SBQPack(testq);
}

int main(int argc, char* argv[]) {
    SBQAllocStatic(testq, 128);

    prepare(testq);
    test1(testq);

	return 0;
}
*/
